#include <ros/ros.h>
#include <fstream>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

#include <tf/transform_broadcaster.h>

#include "eigen_conversions/eigen_msg.h"
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include "path_planner/PathPlanner.h"

using namespace Eigen;
using namespace std;


tf::TransformBroadcaster *tf_uav_enu_pub_ptr;
tf::TransformBroadcaster *tf_posesp_enu_pub_ptr;
bool uav_pose_inited = false;
Affine3d uav_init_pose;


void uavposePoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);

void BroadcastTF(tf::TransformBroadcaster * tfBroadcaster_ptr, const Affine3d& pose,const char* world, const char* tag)
{
	Eigen::Quaterniond pose_R_quat = Eigen::Quaterniond(pose.rotation());
	Eigen::Vector3d pose_t = pose.translation();
	tf::Transform tf_uav_enu;
	tf_uav_enu.setOrigin(tf::Vector3(pose_t.x(),pose_t.y(),pose_t.z()));
	tf_uav_enu.setRotation(tf::Quaternion(
			pose_R_quat.x(),
			pose_R_quat.y(),
			pose_R_quat.z(),
			pose_R_quat.w()
	));
	tfBroadcaster_ptr->sendTransform(tf::StampedTransform(tf_uav_enu,ros::Time::now(), world, tag));
}

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "offboard_controller");
    ros::NodeHandle nh;

    tf_uav_enu_pub_ptr = new tf::TransformBroadcaster();
	tf_posesp_enu_pub_ptr = new tf::TransformBroadcaster();

	ros::Subscriber uavpose_sub = nh.subscribe("/mavros/local_position/pose", 10, uavposePoseCallback);

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Publisher local_pos_sp_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);

    //the setpoint publishing rate MUST be faster than 2Hz
    const double cycle_rate = 40.0;
    const double cycle_interval = 1.0 / cycle_rate;
    ros::Rate rate(cycle_rate);

    // wait for FCU connection
    while(ros::ok() && current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    Affine3d pose_target = Affine3d::Identity();

    pose_target.translation().x() = 0;
    pose_target.translation().y() = 0;
    pose_target.translation().z() = 0.5;

    geometry_msgs::PoseStamped pose_target_msg;

	string file_name = "/home/dozen/path_test.txt";
	ros::param::get("~file_name", file_name);
	ros::param::del("~file_name");

    while(ros::ok()){

    	if(!uav_pose_inited)
    	{
            ros::spinOnce();
            rate.sleep();
            continue;
    	}




    	pose_target_msg.header.stamp = ros::Time::now();
    	tf::poseEigenToMsg(pose_target,pose_target_msg.pose);
    	local_pos_sp_pub.publish(pose_target_msg);
        BroadcastTF(tf_posesp_enu_pub_ptr,pose_target,"ENU","UAV_TARGET");

        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}

void uavposePoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	Affine3d uav_local_pose;
	tf::poseMsgToEigen(msg->pose,uav_local_pose);
	BroadcastTF(tf_uav_enu_pub_ptr,uav_local_pose,"ENU", "UAV");

	if(!uav_pose_inited)
	{
		uav_init_pose=uav_local_pose;
		uav_pose_inited = true;
	}

}



